/*********************************************************************
 * Software License Agreement
 *
 * Copyright (C) 2010 Cross The Road Electronics.  All rights
 * reserved.
 *
 * Cross The Road Electronics (CTRE) licenses to you the right to 
 * use, copy, modify, merge, publish, distribute, sublicense, and/or 
 * sell copies of the Software ONLY when in use with CTRE's 2CAN 
 * Ethernet CAN Gateway.
 *
 * THE SOFTWARE AND DOCUMENTATION ARE PROVIDED "AS IS" WITHOUT
 * WARRANTY OF ANY KIND, EITHER EXPRESS OR IMPLIED, INCLUDING WITHOUT
 * LIMITATION, ANY WARRANTY OF MERCHANTABILITY, FITNESS FOR A
 * PARTICULAR PURPOSE, TITLE AND NON-INFRINGEMENT. IN NO EVENT SHALL
 * CROSS THE ROAD ELECTRONICS BE LIABLE FOR ANY INCIDENTAL, SPECIAL, 
 * INDIRECT OR CONSEQUENTIAL DAMAGES, LOST PROFITS OR LOST DATA, COST OF
 * PROCUREMENT OF SUBSTITUTE GOODS, TECHNOLOGY OR SERVICES, ANY CLAIMS
 * BY THIRD PARTIES (INCLUDING BUT NOT LIMITED TO ANY DEFENSE
 * THEREOF), ANY CLAIMS FOR INDEMNITY OR CONTRIBUTION, OR OTHER
 * SIMILAR COSTS, WHETHER ASSERTED ON THE BASIS OF CONTRACT, TORT
 * (INCLUDING NEGLIGENCE), BREACH OF WARRANTY, OR OTHERWISE.
 *
********************************************************************/
#include "stdafx.h"
#include "2CANClient.h"

#include "xtea.h"
#include "2CANBootloaderDefs.h"
#include "../Common/2CAN_DLL_API.h"
#include <iostream>
#include <stdio.h>
/**
 * Suppress  Warning C4996: 'strcpy' was declared deprecated in Visual Studio.
 */
#ifdef _MSC_VER
#pragma warning(disable : 4996)
#endif

#define ENABLE_HEART_BEAT	(0)
#define VERBOSE				(0)
#define ENABLE_RCM			(1)
#define ENABLE_JAGUAR		(1)


#if VERBOSE > 0
	#define zprintf		printf
#else
	#define zprintf(...) do{}while(0)
#endif



STo2CAN_CANFrame Convert(const CANFrame_t & frm)
{
	STo2CAN_CANFrame retval = {0};
	retval.arbid_h = (frm.arbid >> 16	) & 0xFFFF;
	retval.arbid_l = (frm.arbid			) & 0xFFFF;

	for(uint32_t x=0;x<8;++x)
		retval.data[x] = frm.data[x];

	retval.len_options = ((uint32_t)frm.dlc)<<8;

	retval.len_options |= frm.options;

	return retval;
}

CANFrame_t Convert(const STo2CAN_CANFrame & frm)
{
	CANFrame_t retval = {0};

	retval.arbid = frm.arbid_h;
	retval.arbid <<= 16;
	retval.arbid |= frm.arbid_l;

	for(uint32_t x=0;x<8;++x)
		retval.data[x] = (uint8_t)frm.data[x];

	retval.dlc = frm.len_options >> 8;

	retval.options = frm.len_options & 0xFF;

	return retval;
}

C2CANClient::C2CANClient() : 
	g_StopEvent(L"StopApp",true),
	g_HasStoppedEvent(L"HasStopped",true),
	g_AckReceived(L"AckReceived",true)
{
	memset((void*)g_IP,0,sizeof(g_IP));
	memset((void*)&hb_in,0,sizeof(hb_in));

	g_iPeriod = 1;
	g_task = 0;
			
	InitVars();

	bEnabled = false;
	bSendHB = false;
	heart_beat_timeout = GetTickCount() + 1000;
	_enabledState = Disabled;
	
	inFramesCnt=0;
	inFramesIn=0;
	inFramesOut=0;
}

C2CANClient::~C2CANClient()
{	
	Stop();
}

void C2CANClient::InitVars()
{
	memset((void*)&rcm_out,0,sizeof(rcm_out));
	memset((void*)&rcm_in,0,sizeof(rcm_in));
	memset((void*)&jag_out,0,sizeof(jag_out));
	memset((void*)&enable_out,0,sizeof(enable_out));
	memset((void*)&_commStates,0,sizeof(_commStates));
}
void C2CANClient::ClearInputData()
{
	memset((void*)&rcm_in,0,sizeof(rcm_in));
}
unsigned short CheckSum(unsigned short * data,
						unsigned long byte_len)
{
	unsigned int i=0;
	unsigned short c=0;
	unsigned short d=0;
	
	c = 0;
	for(;i<byte_len/2;++i)
	{
		c += data[i];
	}

	return ~c + 1;
}

#if ENABLE_HEART_BEAT == 1
void C2CANClient::SendHB(SFrom2CAN_Status * pPack)
{
	const uint32_t key[] = {344640930,472568994,868509359,417762980};

	hb_in.iSig = SIG_HEARTBEAT;
	hb_in.iByteLen = sizeof(hb_in);
	hb_in.enabled = 1;

	if(pPack)
	{
		hb_in.response_seed[0] = pPack->seed[0];
		hb_in.response_seed[1] = pPack->seed[1];
		encrypt(hb_in.response_seed,key);
	}
	else
	{
		hb_in.response_seed[0] = 0;
		hb_in.response_seed[1] = 0;
	}


	hb_in.iCRC = 0;
	hb_in.iCRC = CheckSum(hb_in.Words,sizeof(hb_in));

	bSendHB = true;
}
#endif // ENABLE_HEART_BEAT

void C2CANClient::SendRCMOut()
{
	STo2CAN_RCMPacket temp;
	g_sem.Lock();
	temp = rcm_out;
	g_sem.Unlock();

	temp.iSig = SIG_RCM;
	temp.iByteLen = sizeof(temp);
	temp.iCRC = 0;
	temp.iCRC = CheckSum(temp.Words,sizeof(temp));
	
	
	eUDPClientReturnCode_t err = udp_client.Transmit((const uint8_t *)&temp,sizeof(temp),0);
	g_sem.Lock();
	if(err == UDPServer_ReturnCode_Ok)
		++_commStates.txCount;
	else
		++_commStates.txCountFailed;
	g_sem.Unlock();
}

void C2CANClient::SendEnableOut()
{
	STo2CAN_EnablePacket temp = {0};
	g_sem.Lock();
	++enable_out.sequence;
	temp = enable_out;
	g_sem.Unlock();

	temp.iSig = SIG_ENABLE;
	temp.iByteLen = sizeof(temp);
	temp.iCRC = 0;
	temp.iCRC = CheckSum(temp.Words,sizeof(temp));
	
	CUDPClient::eReturnCode_t err = udp_client.Transmit((const uint8_t *)&temp,sizeof(temp),0);
	g_sem.Lock();
	if(err == UDPServer_ReturnCode_Ok)
		++_commStates.txCount;
	else
		++_commStates.txCountFailed;
	g_sem.Unlock();
}

void C2CANClient::SendJagOut()
{
	STo2CAN_UpdateJagPacket temp;
	g_sem.Lock();
	temp = jag_out;
	g_sem.Unlock();

	temp.iSig = SIG_UPDATE_JAG_PACKET;
	temp.iByteLen = sizeof(temp);
	temp.iCRC = 0;
	temp.iCRC = CheckSum(temp.Words,sizeof(temp));
	
	eUDPClientReturnCode_t err = udp_client.Transmit((const uint8_t *)&temp,sizeof(temp),0);
	g_sem.Lock();
	if(err == UDPServer_ReturnCode_Ok)
		++_commStates.txCount;
	else
		++_commStates.txCountFailed;
	g_sem.Unlock();
}
void C2CANClient::SendFrameOut(const STo2CAN_CANFrame & canframe)
{
	STo2CAN_TxCANPacket temp = {0};

	temp.CANFrames[0] = canframe;
	temp.iFrameCnt = 1;

	temp.iSig = SIG_TX_CAN_FRAMES;
	temp.iByteLen = sizeof(temp);
	temp.iCRC = 0;
	temp.iCRC = CheckSum(temp.Words,sizeof(temp));
	
	eUDPClientReturnCode_t err = udp_client.Transmit((const uint8_t *)&temp,sizeof(temp),0);
	g_sem.Lock();
	if(err == UDPServer_ReturnCode_Ok)
		++_commStates.txCount;
	else
		++_commStates.txCountFailed;
	g_sem.Unlock();
}

bool C2CANClient::SendFilterUpdate(	unsigned int iFilterIdx,
						STo2CAN_CANFrame & value,
						STo2CAN_CANFrame & mask)
{
	g_AckReceived.Reset();

	STo2CAN_FilterCANPacket filter = {0};

	filter.iFilterIdx = iFilterIdx;
	filter.mask = mask;
	filter.value = value;

	filter.iSig = SIG_SET_CAN_FILTER;
	filter.iByteLen = sizeof(filter);
	filter.iCRC = 0;
	filter.iCRC = CheckSum(filter.Words,sizeof(filter));
	
	CUDPClient::eReturnCode_t err = udp_client.Transmit((const uint8_t *)&filter,sizeof(filter),0);
	g_sem.Lock();
	if(err == UDPServer_ReturnCode_Ok)
		++_commStates.txCount;
	else
		++_commStates.txCountFailed;
	g_sem.Unlock();

	bool bRet = g_AckReceived.WaitUntilSignaled(500);

	return bRet;
}

int C2CANClient::ProcessRxData(CUDPClient & client,const uint8_t * pData, uint32_t ulNumBytes)
{
	SFrom2CAN_Status * pPack = (SFrom2CAN_Status * )pData;

#if ENABLE_HEART_BEAT == 1
	//heart beat commands....
	if(&client == &udp_hb)
	{
		if(pPack->iSig == SIG_STATUS)
		{
			SendHB( (SFrom2CAN_Status *) pPack);

			heart_beat_timeout = GetTickCount() + 1000;

			return 1;
		}
		return 0;
	}
#endif
		
	//everything else...
	switch(pPack->iSig)
	{
		case SIG_RCM_STATUS:
		{
			SFrom2CAN_RCMPacket * pPack = (SFrom2CAN_RCMPacket * )pData;

			g_sem.Lock();
			rcm_in = *pPack;
			++_commStates.rxCount;
			_commStates.rxTimeFromLastMs = 0;
			g_sem.Unlock();
		}break;

		case SIG_ACK:
		{
			SFrm2CAN_AckPacket * pPack = (SFrm2CAN_AckPacket * )pData;

			g_AckReceived.Signal();
		}break;

		case SIG_RX_CAN_FRAMES:
		{
			STo2CAN_RxCANPacket * pPack = (STo2CAN_RxCANPacket * )pData;
			
			uint32_t num = min( inFramesCap - inFramesCnt , pPack->iFrameCnt );

			for(uint32_t i=0;i<num;++i)
			{
				inFrames[inFramesIn] = Convert(pPack->CANFrames[i]);
				inFramesIn = (inFramesIn + 1) % inFramesCap;
				++inFramesCnt;
			}

		}break;
	}
	return 0;
}

int C2CANClient::EmptyReceiveData(CUDPClient & client)
{
	while(client.HasData())
	{
		uint8_t resp[2048];
		uint32_t ulIP;
		uint32_t ulNumBytes = client.Read(resp,sizeof(resp),ulIP);
		if( CheckSum((unsigned short*)resp,ulNumBytes) != 0)
		{
			zprintf("bad chksum\r\n");
		}
		else 
		{
			ProcessRxData(client,resp,ulNumBytes);
			

		}
	}
	return 0;
}
void C2CANClient::ProcessUDP()
{
	static int counter = 0;

	EmptyReceiveData(udp_hb);

#if ENABLE_HEART_BEAT == 1
	if(bEnabled)
		if( (heart_beat_timeout- (signed long)GetTickCount()) < 0 )
		{
			SendHB(0);

			heart_beat_timeout = GetTickCount() + 1000;

			zprintf("Attempting to sync heartbeat (%d)\r",counter++ );
		}
#endif

	EmptyReceiveData(udp_client);
}

void C2CANClient::Process10Ms()
{
	if(bEnabled)
	{
#if ENABLE_HEART_BEAT == 1
		if(bSendHB)
		{
			bSendHB = false;

			udp_hb.Transmit(  (const uint8_t *)&hb_in,sizeof(hb_in),0);
		}
#endif
	}

#if ENABLE_RCM == 1
	SendRCMOut();
#endif
#if ENABLE_JAGUAR == 1
	SendJagOut();
#endif
	SendEnableOut();
}


unsigned int C2CANClient::bt2CAN_RC_StaticThread(void * lpParam)
{
	return ((C2CANClient*)lpParam)->bt2CAN_RC_Thread();
}
unsigned int C2CANClient::bt2CAN_RC_Thread()
{
	char buffer[100] = {0};
	signed long iTicks10Ms = 10;

	buffer[0] = 0;
	memset(jag_out.uiMode,0xFF,sizeof(jag_out.uiMode));
	strcpy(buffer,g_IP);

	udp_client.SetOutRemotePort(1217).SetInLocalPort(1218).SetRemoteIP(buffer);
	udp_client.Connect();

	udp_hb.SetOutRemotePort(1219).SetInLocalPort(1220).SetRemoteIP(buffer);
	udp_hb.Connect();

	iTicks10Ms = GetTickCount()-1000;
	while(g_StopEvent.IsSignaled() == false)
	{

		if( (signed long)(GetTickCount() - iTicks10Ms) > 0)
		{
			iTicks10Ms = GetTickCount() + 10;

			Process10Ms();

			g_sem.Lock();
			if(_commStates.rxTimeFromLastMs < 60000) // cap at 1 min
				_commStates.rxTimeFromLastMs += 10;
			if(_commStates.rxTimeFromLastMs > 150)
			{
				_commStates.statusFlags |= CommStatsNoResponse;
				ClearInputData();
			}
			else
				_commStates.statusFlags &= ~CommStatsNoResponse;
			g_sem.Unlock();
		}

		ProcessUDP();

		Sleep(g_iPeriod);
	}

	g_HasStoppedEvent.Signal();
	return 0;
}

ctr_error_t C2CANClient::Find2Can(Found2Can * found2can,uint32_t options)
{
	CUDPClient udp_broadcast;
	char destIp[20];
	unsigned long len=0;
	uint32_t ulIP;  

	strcpy(destIp, "255.255.255.255");
	udp_broadcast.SetOutRemotePort(32000).SetInLocalPort(32001).SetRemoteIP(destIp);
	udp_broadcast.Connect();
	if(udp_broadcast.WaitUntilConnected(500) == false)
		return ctr_fail;

	const uint8_t whos_there[] = { 0xcc, 0xaa, 0x00, 0x00, 0x00, 0x00, 0x8a, 0x80, 0x00,
			0x00, 0xaa, 0xd4, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };

	if( udp_broadcast.Transmit((const uint8_t*)whos_there, sizeof(whos_there),0) != UDPServer_ReturnCode_Ok)
		return ctr_fail;

	if( udp_broadcast.WaitForRxData(500) == false)
		return ctr_fail;
	if( udp_broadcast.HasData() == false)
		return ctr_fail;

	BL_COMM_Resp rx;
	int readCnt = 0;
	while(udp_broadcast.HasData())
	{
		len = udp_broadcast.Read((uint8_t*)&rx, sizeof(rx),ulIP);
		if (len == 72)
			break;
		++readCnt;
		if(readCnt > 100)
			break;
	}
	udp_broadcast.Disconnect();

	int packetHeaderSize = sizeof(rx) - sizeof(rx.iAppPacketBytes);
	int payloadSize = sizeof(BL_COMM_RxPacketInfo);
	if (len != (packetHeaderSize + payloadSize))
		return ctr_nodeNotFound;

	
	BL_COMM_RxPacketInfo *packet = (BL_COMM_RxPacketInfo *)rx.iAppPacketBytes;
	found2can->iBootloaderMajor = packet->iBootloaderMajor;
	found2can->iBootloaderMinor = packet->iBootloaderMinor;
	found2can->iFirmwareMajor = packet->iFirmwareMajor;
	found2can->iFirmwareMinor = packet->iFirmwareMinor;
	found2can->iManDay = packet->iManDay;
	found2can->iManMonth = packet->iManMonth;
	found2can->iManYear = packet->iManYear;
	found2can->iHardwareRevMajor = packet->iHardwareRevMajor;
	found2can->iHardwareRevMinor = packet->iHardwareRevMinor;
	found2can->ip[0] = (uint8_t)packet->uiIP[0];
	found2can->ip[1] = (uint8_t)packet->uiIP[1];
	found2can->ip[2] = (uint8_t)packet->uiIP[2];
	found2can->ip[3] = (uint8_t)packet->uiIP[3];
	found2can->ip[4] = (uint8_t)packet->uiIP[4];
	found2can->ip[5] = (uint8_t)packet->uiIP[5];
	return ctr_ok;
}

ctr_error_t C2CANClient::Init(const char * ip)
{
	strcpy(g_IP,ip);
	g_StopEvent.Reset();
	g_HasStoppedEvent.Reset();
	if(g_task)
		delete g_task;

	InitVars();

	g_task = new OS_Task(L"Application",bt2CAN_RC_StaticThread,this);

	bool connected = udp_client.WaitUntilConnected(500);
	if(connected)
		connected = udp_hb.WaitUntilConnected(10);

	if(!connected)
	{
		return ctr_fail;
	}

	return ctr_ok;
}
ctr_error_t C2CANClient::SetCommPeriod(uint32_t iPeriod)
{
	g_iPeriod = iPeriod;
	return ctr_ok;
}
ctr_error_t C2CANClient::Stop()
{
	//Signal background thread to stop
	g_StopEvent.Signal();
	//wait for thread to signal back that it has stopped
	bool bStopped = g_HasStoppedEvent.WaitUntilSignaled(500);
	//delete task
	if(g_task)
		delete g_task;
	g_task = 0;
	//now stop the udp clients
	udp_client.Disconnect();
	udp_hb.Disconnect();

	return ctr_ok;
}
ctr_error_t C2CANClient::SetActiveRcmNodeid(uint16_t nodeId)
{
	g_sem.Lock();
	rcm_out.iRcmNodeId = (uint8_t)nodeId;
	g_sem.Unlock();
	return ctr_ok;
}
ctr_error_t C2CANClient::Enable(EnableState enableState,uint64_t outputEnableBits)
{
	_enabledState = enableState;
	bEnabled = (_enabledState != Disabled) ? (true) : (false);
	
	g_sem.Lock();
	enable_out.enableState = (uint8_t)enableState;
	enable_out.outputEnables = outputEnableBits;
	g_sem.Unlock();

	return ctr_ok;
}
ctr_error_t C2CANClient::IsEnabled(uint32_t *is_enabled)
{
	*is_enabled = (bEnabled) ? 1 : 0;
	return ctr_ok;
	
}

ctr_error_t C2CANClient::SetPWM(ctr_nodeType_t node,int param,int channel, int16_t pulseWidth)
{
	switch(node)
	{	
		case ctr_Jaguar:
			g_sem.Lock();
			jag_out.uiSetVoltage[channel] = pulseWidth;
			jag_out.uiMode[channel] = 0;
			g_sem.Unlock();
			return ctr_ok;

		case ctr_RCM:
			//todo use param as rcmNodeId
			g_sem.Lock();
			rcm_out.iPWM[channel] = pulseWidth;
			rcm_out.iRcmNodeId = param;
			g_sem.Unlock();
			return ctr_ok;
	}
	return ctr_badNodeType;
}
ctr_error_t C2CANClient::GetADC(ctr_nodeType_t node,int param,int channel,uint32_t * result)
{
	g_sem.Lock();
	int retval = rcm_in.analog_in[channel];
	g_sem.Unlock();
	*result = retval;
	return ctr_ok;
}
ctr_error_t C2CANClient::GetPosition(ctr_nodeType_t node,int param,int channel,uint32_t * result)
{
	g_sem.Lock();
	int retval = rcm_in.quad_in[channel];
	g_sem.Unlock();
	*result = retval;
	return ctr_ok;
}
ctr_error_t C2CANClient::GetVelocity(ctr_nodeType_t node,int param,int channel,uint32_t * result)
{	
	g_sem.Lock();
	int retval = rcm_in.velocity_in[channel];
	g_sem.Unlock();
	*result = retval;
	return ctr_ok;
}    
ctr_error_t C2CANClient::SetSolenoid(ctr_nodeType_t node,int param,int channel, bool enable)
{
	g_sem.Lock();
	rcm_out.iSolenoid[channel] = enable;
	rcm_out.iRcmNodeId = param;
	g_sem.Unlock();
	return ctr_ok;
}    
ctr_error_t C2CANClient::SetRelay(ctr_nodeType_t node,int param,int channel, unsigned char state)
{
	g_sem.Lock();
	rcm_out.iRelay[channel] = state;
	rcm_out.iRcmNodeId = param;
	g_sem.Unlock();
	return ctr_ok;
}   

ctr_error_t C2CANClient::GetGPIO(ctr_nodeType_t node,int param,uint32_t * result)
{
	unsigned int retval = 0;
	g_sem.Lock();
	retval = rcm_in.GPIO_IN;
	g_sem.Unlock();
	*result = retval;
	return ctr_ok;
}

ctr_error_t C2CANClient::SetFilter(	unsigned int iFilterIdx,
								const CANFrame_t * pMsgData,
								const CANFrame_t * pMsgMask)
{
	STo2CAN_CANFrame value = Convert(*pMsgData);
	STo2CAN_CANFrame mask = Convert(*pMsgMask);

	if(SendFilterUpdate(iFilterIdx,value,mask))
		return ctr_ok;
	return ctr_fail;
}  

ctr_error_t C2CANClient::ClearFilter(unsigned int iFilterIdx)
{
	g_AckReceived.Reset();

	STo2CAN_CmdPacket packet = {0};

	packet.iData[0] = iFilterIdx;
	packet.iSig = SIG_CLEAR_CAN_FILTER;
	packet.iByteLen = sizeof(packet);
	packet.iCRC = 0;
	packet.iCRC = CheckSum(packet.Words,sizeof(packet));
	
	CUDPClient::eReturnCode_t err = 
		udp_client.Transmit(	(const uint8_t *)&packet,
								sizeof(packet),0);
	g_sem.Lock();
	if(err == UDPServer_ReturnCode_Ok)
		++_commStates.txCount;
	else
		++_commStates.txCountFailed;
	g_sem.Unlock();

	bool bRet = g_AckReceived.WaitUntilSignaled(500);

	if(bRet)
		return ctr_ok;
	return ctr_fail;
}

ctr_error_t C2CANClient::GetFrames(	CANFrame_t * frames,
									uint32_t capacity,
									uint32_t * num_msgs)
{
	uint32_t num = min( inFramesCnt , capacity);

	if(num_msgs)
		*num_msgs = 0;

	for(uint32_t i=0;i<num;++i)
	{
		frames[i] = inFrames[inFramesOut];
		inFramesOut = (inFramesOut + 1) % inFramesCap;
		--inFramesCnt;
	}

	if(num_msgs)
		*num_msgs = num;

	if(!num_msgs)
		return ctr_fail;

	return ctr_ok;
}

ctr_error_t C2CANClient::SendFrames(	const CANFrame_t * frames,
										uint32_t num_msgs)
{
	for(uint32_t i=0;i<num_msgs;++i)
		SendFrameOut(Convert(frames[i]));

	return ctr_ok;
}

ctr_error_t C2CANClient::GetCommStats(CommStats * stats)
{
	if(!stats)
		return ctr_badParameter;
	g_sem.Lock();
	*stats = _commStates;
	g_sem.Unlock();
	return ctr_ok;
}


